#include "device_imageElement.h"
#include <stdint.h>
#include <math.h>
#include <stdbool.h>


// 最小二乘法线性拟合
Line fitLine(uint8_t *points, uint16_t start, uint16_t end) {
    Line line = {0, 0};
    uint16_t n = end - start + 1;
    if (n < 2) return line;

    float sumX = 0, sumY = 0, sumXY = 0, sumX2 = 0;
    for (uint16_t i = start; i <= end; i++) {
        sumX += i;
        sumY += points[i];
        sumXY += i * points[i];
        sumX2 += i * i;
    }

    float denominator = n * sumX2 - sumX * sumX;
    if (fabs(denominator) < 1e-6) {
        line.slope = 0;
    } else {
        line.slope = (n * sumXY - sumX * sumY) / denominator;
    }
    line.intercept = (sumY - line.slope * sumX) / n;

    return line;
}

// 计算两线交点
bool calculateIntersection(Line l1, Line l2, float *x, float *y) {
    if (fabs(l1.slope - l2.slope) < 1e-4) {
        return false;
    }

    *x = (l2.intercept - l1.intercept) / (l1.slope - l2.slope);
    *y = l1.slope * (*x) + l1.intercept;
    return true;
}
// 检测三线是否相交
bool detectRealIntersection(uint8_t *l_border, uint8_t *r_border, 
                            uint8_t *center_line) {
    const float MAX_ANGLE_DEG = 75.0f;       // 放宽角度限制
    const float MAX_ANGLE_RAD = MAX_ANGLE_DEG * M_PI / 180.0f;
    const uint16_t SEGMENT_SIZE = 15;        // 增大拟合窗口
    const float INTERSECT_TOL = 10.0f;       // 放宽交点一致性
    const float SLOPE_TOL = 0.2f;            // 放宽斜率趋势
    
    // 尝试多个尺度
    for (uint8_t scale = 1; scale <= 3; scale++) {
        uint16_t currentSeg = SEGMENT_SIZE * scale;
        if (currentSeg > image_h / 2) break;
        
        for (uint16_t y = 50; y <= image_h - currentSeg; y += currentSeg/2) {
            uint16_t end = y + currentSeg - 1;
            if (end >= image_h) end = image_h - 1;

            Line leftLine = fitLine(l_border, y, end);
            Line centerLine = fitLine(center_line, y, end);
            Line rightLine = fitLine(r_border, y, end);

            float xLc, yLc, xLr, yLr, xCr, yCr;
            bool hasLc = calculateIntersection(leftLine, centerLine, &xLc, &yLc);
            bool hasLr = calculateIntersection(leftLine, rightLine, &xLr, &yLr);
            bool hasCr = calculateIntersection(centerLine, rightLine, &xCr, &yCr);

            if (!hasLc || !hasLr || !hasCr) continue;
            if (yLc < 0 || yLc >= image_h || yLr < 0 || yLr >= image_h || 
                yCr < 0 || yCr >= image_h) continue;

            // 交点一致性校验
            float dx1 = fabs(xLc - xLr);
            float dy1 = fabs(yLc - yLr);
            float dx2 = fabs(xLc - xCr);
            float dy2 = fabs(yLc - yCr);
            float avgDist = (dx1 + dy1 + dx2 + dy2) / 4.0f;
            if (avgDist > INTERSECT_TOL) continue;

            // 简化的趋势校验：左线和右线斜率符号相反
            bool leftTrend = (leftLine.slope > 0);
            bool rightTrend = (rightLine.slope < 0);
            bool validSlope = (fabs(leftLine.slope) > SLOPE_TOL && 
                              fabs(rightLine.slope) > SLOPE_TOL);
            if (!leftTrend || !rightTrend || !validSlope) continue;

            // 角度校验（放宽限制）
            float angleLc = fabs(atan2(1, leftLine.slope) - atan2(1, centerLine.slope));
            float angleLr = fabs(atan2(1, leftLine.slope) - atan2(1, rightLine.slope));
            float angleCr = fabs(atan2(1, centerLine.slope) - atan2(1, rightLine.slope));
            if (angleLc > MAX_ANGLE_RAD && angleLr > MAX_ANGLE_RAD && 
                angleCr > MAX_ANGLE_RAD) continue; // 至少两个角度符合即可

            return true; // 找到符合条件的交点
        }
    }

    return false;
}

#include <stdio.h>

// 齿轮参数定义
#define MOTOR_GEAR_TEETH 11       // 电机齿轮齿数
#define WHEEL_GEAR_TEETH 58       // 轮子齿轮齿数
#define ENCODER_GEAR_TEETH 30     // 编码器齿轮齿数
#define ENCODER_PULSES_PER_REV 1024  // 编码器每转脉冲数
#define WHEEL_DIAMETER_M 0.0495   // 车轮直径(米)
#define PI 3.14159265358979323846

/**
 * 根据编码器脉冲计算小车速度和行驶距离
 * @param left_pulses 左轮累计脉冲数
 * @param right_pulses 右轮累计脉冲数
 * @param motor_speed_rpm 电机转速(RPM)
 * @param left_speed 左轮速度(输出，米/秒)
 * @param right_speed 右轮速度(输出，米/秒)
 * @param left_distance 左轮行驶距离(输出，米)
 * @param right_distance 右轮行驶距离(输出，米)
 */
void calculate_vehicle_motion(int left_pulses, int right_pulses, float motor_speed_rpm,
                             float *left_speed, float *right_speed,
                             float *left_distance, float *right_distance) {
    // 计算车轮周长(米)
    float wheel_circumference = PI * WHEEL_DIAMETER_M;
    
    // 计算齿轮传动比
    float motor_to_wheel_ratio = (float)MOTOR_GEAR_TEETH / WHEEL_GEAR_TEETH;
    float encoder_to_wheel_ratio = (float)ENCODER_GEAR_TEETH / WHEEL_GEAR_TEETH;
    
    // 计算每脉冲对应的距离(米/脉冲)
    float distance_per_pulse = wheel_circumference / (ENCODER_PULSES_PER_REV * encoder_to_wheel_ratio);
    
    // 计算行驶距离(米)
    *left_distance = left_pulses * distance_per_pulse;
    *right_distance = right_pulses * distance_per_pulse;
    
    // 计算车轮转速(RPM)
    float wheel_speed_rpm = motor_speed_rpm * motor_to_wheel_ratio;
    
    // 计算线速度(米/秒)
    float wheel_speed_m_per_min = wheel_circumference * wheel_speed_rpm;
    *left_speed = wheel_speed_m_per_min / 60.0;  // 转换为米/秒
    *right_speed = *left_speed;  // 假设左右轮速度相同(直线行驶)
}

// 示例使用
void displayCarParam()
{
    int left_pulses = g_encoder_left;
    int right_pulses = g_encoder_right;
    float motor_speed = 17066.0;  // 电机空转转速(RPM)
    float left_speed, right_speed, left_distance, right_distance;
    
    calculate_vehicle_motion(left_pulses, right_pulses, motor_speed,
                            &left_speed, &right_speed,
                            &left_distance, &right_distance);
    
    printf("左轮脉冲数: %d\n", left_pulses);
    printf("右轮脉冲数: %d\n", right_pulses);
    printf("电机转速: %.2f RPM\n", motor_speed);
    printf("左轮速度: %.4f m/s\n", left_speed);
    printf("右轮速度: %.4f m/s\n", right_speed);
    printf("左轮行驶距离: %.4f m\n", left_distance);
    printf("右轮行驶距离: %.4f m\n", right_distance);
}

extern uint16 timerCounter;
//测试小车行驶的距离
void testMotor()
{

}